{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "7762616d",
      "metadata": {},
      "source": [
        "# Fisheye Canera Structure-From-Motion Example\n",
        "\n",
        "A visualSLAM example for the structure-from-motion problem on a\n",
        "simulated dataset. This version uses a fisheye camera model and a GaussNewton\n",
        "solver to solve the graph in one batch."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "colab_button",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/FisheyeExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "cbf9c44c",
      "metadata": {},
      "source": [
        "## Problem Description\n",
        "\n",
        "A structure-from-motion example with landmarks:\n",
        "- The landmarks form a 10 meter cube\n",
        "- The robot rotates around the landmarks, always facing towards the cube\n",
        "\n",
        "This example demonstrates:\n",
        "- Camera observations of landmarks (pixel coordinates) stored as Point2 (x, y)\n",
        "- Each variable (poses and landmarks) identified with unique keys using Symbols\n",
        "- Projection factors to model camera's landmark observations\n",
        "- Prior factors to initialize robot location\n",
        "- Fisheye camera model (Cal3Fisheye) with distortion parameters"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "051ae843",
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "\n",
        "import gtsam\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "L = symbol_shorthand.L\n",
        "X = symbol_shorthand.X\n",
        "\n",
        "from gtsam.examples import SFMdata\n",
        "\n",
        "from gtsam import (Cal3Fisheye, GaussNewtonOptimizer, GaussNewtonParams,\n",
        "                   GenericProjectionFactorCal3Fisheye,\n",
        "                   NonlinearFactorGraph, PinholeCameraCal3Fisheye,\n",
        "                   Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,\n",
        "                   Rot3, Values)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "11bb20b0",
      "metadata": {},
      "source": [
        "## Camera Setup, Data Generation"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "0d6af772",
      "metadata": {},
      "outputs": [],
      "source": [
        "K = Cal3Fisheye(\n",
        "    278.66, 278.48, 0.0, 319.75, 241.96,\n",
        "    -0.013721808247486035, 0.020727425669427896,\n",
        "    -0.012786476702685545, 0.0025242267320687625\n",
        ")\n",
        "\n",
        "# Define the camera observation noise model, 1 pixel stddev\n",
        "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "\n",
        "# Create the set of ground-truth landmarks\n",
        "points = SFMdata.createPoints()\n",
        "\n",
        "# Create the set of ground-truth poses\n",
        "poses = SFMdata.createPoses()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "0fbe712b",
      "metadata": {},
      "source": [
        "## Factor Graph Setup"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "b6891424",
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create a Factor Graph and Values to hold the new data\n",
        "graph = NonlinearFactorGraph()\n",
        "initial_estimate = Values()\n",
        "\n",
        "# Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z\n",
        "pose_prior = gtsam.noiseModel.Diagonal.Sigmas(\n",
        "    np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])\n",
        ")\n",
        "graph.add(PriorFactorPose3(X(0), poses[0], pose_prior))\n",
        "\n",
        "# Add a prior on landmark l0\n",
        "point_prior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)\n",
        "graph.add(PriorFactorPoint3(L(0), points[0], point_prior))\n",
        "\n",
        "# Add initial guesses to all observed landmarks\n",
        "# Intentionally initialize the variables off from the ground truth\n",
        "delta_point = Point3(-0.25, 0.20, 0.15)\n",
        "for j, point in enumerate(points):\n",
        "    initial_estimate.insert(L(j), point + delta_point)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "c98d4fa8",
      "metadata": {},
      "source": [
        "## Add observations, initial estimates"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "99d0b6ca",
      "metadata": {},
      "outputs": [],
      "source": [
        "for i, pose in enumerate(poses):\n",
        "    # Add factors for each landmark observation\n",
        "    for j, point in enumerate(points):\n",
        "        camera = PinholeCameraCal3Fisheye(pose, K)\n",
        "        measurement = camera.project(point)\n",
        "        factor = GenericProjectionFactorCal3Fisheye(\n",
        "            measurement, measurement_noise, X(i), L(j), K\n",
        "        )\n",
        "        graph.add(factor)\n",
        "\n",
        "    # Add an initial guess for the current pose\n",
        "    # Intentionally initialize the variables off from the ground truth\n",
        "    delta_pose = Pose3(\n",
        "        Rot3.Rodrigues(-0.1, 0.2, 0.25),\n",
        "        Point3(0.05, -0.10, 0.20)\n",
        "    )\n",
        "    initial_estimate.insert(X(i), pose.compose(delta_pose))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "1afe5494",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Optimizing the factor graph\n",
            "converged\n",
            "errorThreshold: Optimization complete\n",
            "4.55607e-21 <? 0\n",
            "absoluteDecrease: 6.26303251472e-08 <? 1e-05\n",
            "relativeDecrease: 1 <? 1e-05\n",
            "iterations: 4 >? 10000\n"
          ]
        }
      ],
      "source": [
        "# Configure and run the optimizer\n",
        "params = GaussNewtonParams()\n",
        "params.setVerbosity(\"TERMINATION\")\n",
        "params.setMaxIterations(10000)\n",
        "\n",
        "print(\"Optimizing the factor graph\")\n",
        "optimizer = GaussNewtonOptimizer(graph, initial_estimate, params)\n",
        "result = optimizer.optimize()\n",
        "print(\"Optimization complete\")"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "7a125857",
      "metadata": {},
      "source": [
        "## Results"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "20e55091",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "initial error=152507.22412516878\n",
            "final error=4.5560711028681596e-21\n",
            "Saved graph to python/gtsam/examples/fisheye_batch.dot\n"
          ]
        }
      ],
      "source": [
        "print(f\"initial error={graph.error(initial_estimate)}\")\n",
        "print(f\"final error={graph.error(result)}\")\n",
        "\n",
        "# Save the factor graph visualization\n",
        "try:\n",
        "    graph.saveGraph(\"python/gtsam/examples/fisheye_batch.dot\", result)\n",
        "    print(\"Saved graph to python/gtsam/examples/fisheye_batch.dot\")\n",
        "except Exception as e:\n",
        "    print(f\"Could not save graph: {e}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": ".venv",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.9"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}